#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
    需求：发布两个坐标系之间的相对关系 
        1.包含头文件 
        2.设置编码、节点初始化 NodeHandle 
        3.创建发布对象
        4.组织被发布的消息
        5.发布数据；(静态坐标变换，只要发布一次 就可以了)
        6.spin();
 */


int main(int argc, char  *argv[])
{
        // 2.设置编码、节点初始化 NodeHandle 
        setlocale(LC_ALL, "");
        ros::init(argc, argv, "static_pub");
        ros::NodeHandle nh; 

        // 3.创建发布对象
        tf2_ros::StaticTransformBroadcaster broadcaster;

        // 4.组织被发布的消息
        geometry_msgs::TransformStamped tfs;
        tfs.header.stamp = ros::Time::now();
        tfs.header.frame_id = "base_link";  // 相对坐标关系中被参数的那一个 
        tfs.child_frame_id = "laser";
        tfs.transform.translation.x = 0.2;
        tfs.transform.translation.y = 0;
        tfs.transform.translation.z = 0.5; 

        // 需要根据欧拉角转换
        tf2::Quaternion qtn;    //创建 四元数 对象 
        // 向该对应设置敬欧拉角，这个对象可以 将欧拉角转换成四元数
        qtn.setRPY(0,0,0);   // 欧拉角的单位是弧度

        tfs.transform.rotation.x =  qtn.getX();
        tfs.transform.rotation.y =  qtn.getY();
        tfs.transform.rotation.z =  qtn.getZ();
        tfs.transform.rotation.w =  qtn.getW();
        

        // 5.发布数据；(静态坐标变换，只要发布一次 就可以了)
        broadcaster.sendTransform(tfs);

        // 6. spin();
        ros::spin();
    return 0;
}
